package allparser;

import java.io.*;
import java.math.*;
import java.util.*;

public class AllFile {
    
    public class Datagram68{
        public double[] X;
        public double[] angleX;
        public double[] Y;
        public double[] Z;
        public double[] quality;
        public double[] BSS;
        public Pozycja pozycja;
        public int beamCount;

        
        public Datagram68(int noOfValidBeams){
            beamCount = noOfValidBeams;
            
            X = new double[beamCount];
            angleX = new double[beamCount];
            Y = new double[beamCount];
            Z = new double[beamCount];
            quality = new double[beamCount];
            BSS = new double[beamCount];         
        }
    }
    
    public List<Datagram68> datagram68;
    
    public AllFile(int pocz_datagram, int ilosc_datagramow, String filename) throws IOException {
        
        //deklaracje
        datagram68 = new ArrayList<Datagram68>();
        Pozycja tpozycja = new Pozycja();
        // open file
        URandomAccessFile f = new URandomAccessFile(filename, "r");

        f.seek(6);
        f.setEndian('l');
        int emType = f.readUShort();
        // allowed emType
        if( emType != 300 && emType != 120 && emType != 3002 && emType != 3020)
            f.setEndian('b');

        int noOfHeads = 2;
        if( emType == 3002 || emType == 3003 || emType == 3004 || emType == 3005 
                || emType == 3006 || emType == 3007 || emType == 3008 || emType == 3020)
            noOfHeads = 2;
        else
            noOfHeads = 1;

        f.seek(0);
        // -------------------------------------------------------------------------
        //numer pingu
        int i=1;

        //index ilosci datgramow na dany swath
        int ind = 0;

        int previous_pn = -1; //numer ostatniego pingu

        // -------------------------------------------------------------------------

        int datagramType = 0;
        int beam_nr=0;
        int licznik_datagramow=0;
        int licznik_datagramow_overall = 0;
        long dlugosc_pliku = f.length();
        long datagramLength = 0;
        long filepointer = 0;
        int ss, dp, nb; 
        double sx = -100, sy = 0;
        double hd = 0, cv, th, ux = 0, uy = 0, sv = 0;
        long staretm = 0;
        
        
        boolean jestpozycja = false;
        //----------------------------------------------------------------------
        while (f.getFilePointer()<dlugosc_pliku&&licznik_datagramow<=ilosc_datagramow){
    
            // read next datagram datagram length
            datagramLength = f.readUInt();
            licznik_datagramow_overall++;
            if (f.getFilePointer()>=dlugosc_pliku)
                break;
            filepointer = f.getFilePointer();
    
            // skip stx
            short stx = f.readUByte();
            if(stx != 2){
//                System.out.println("fail z stx");
                f.close();
                return;
            }

            // datagram type
            datagramType =  ((f.readUByte()));
    
            //serial number
            int sn   = f.readUShort();
            //date
            long date = f.readUInt();
            //time
            long tm = f.readUInt();
    
            //wybor datagramu
            switch(datagramType){
                case 68: //Depth datagram hex 44
                    if((licznik_datagramow_overall > pocz_datagram)&&jestpozycja){
//                        System.out.println("Datagram "+licznik_datagramow_overall);
                        
                        licznik_datagramow++;
                        //timer(licznik_datagramow) = tm;
                
                        f.seek(f.getFilePointer()+2); 
                        //serial number
                        sn = f.readUShort();
                
                        //heading
                        hd = f.readUShort();
                        hd = hd / 100;

                        //sound speed
                        ss = f.readUShort();
                
                        //transmit transducer depth relative to water level at time
                        //of ping in cm
                        dp = f.readUShort();
                        nb = f.readUByte();
                        
                        double dt = 0;
                        if (tm > staretm)
                          dt = (1.0 * ((tm - staretm)) / 1000);
                        th = (-hd *  Math.PI / 180);
                
                        int noOfValidBeams = f.readUByte();
                        //System.out.println(noOfValidBeams);
                        
                        int zResolution = f.readUByte();
                        int xyResolution = f.readUByte();
                        int sampling_rate = f.readUShort();
                
                        Datagram68 datagram = new Datagram68(noOfValidBeams);
                        
                        for(int j = 0; j < noOfValidBeams; j++){
                    
                            double gl = f.readSShort();
                            //gl = (gl + dp);
                            
                            double Yr = f.readSShort();
                            double Xr = f.readSShort();
                            
                            double z_ = (1.0 * gl / 100); 
                            double y_ = (double)(1.0 * Yr / 100);
                            double x_ = (double)(1.0 * Xr / 100);
                            
                            datagram.Z[j] = z_ ; 
                            datagram.Y[j] = y_ ;
                            datagram.X[j] = x_ ;
                            //datagram.X[j] = y_ * Math.cos(th) - x_ * Math.sin(th) + dt * sv * Math.sin(-th) + ux - sx;
                            //datagram.Y[j] = y_ * Math.sin(th) + x_ * Math.cos(th) + dt * sv * Math.cos(-th) + uy - sy;
                            datagram.angleX[j] = Math.atan(datagram.Y[j]/datagram.Z[j]);
                    
                            //skip range, quality factor, detection window...
                            int beamDepresionAngle = f.readSShort();
                    
                            int beamAzimuth = f.readSShort();
                    
                            f.seek(f.getFilePointer()+2); 
                            datagram.quality[j] = f.readUByte();
                            f.seek(f.getFilePointer()+1); 
                            //w dokumentacji jest niejasno napisane ale wyszlo nam
                            //ze tak jak tutaj liczymy jest OK
                            datagram.BSS[j] = (double)(f.readUByte()-256)/2;
                    
                            int beamNumber =  f.readUByte();
                        }
                        datagram.pozycja = tpozycja;
                        datagram68.add(datagram);
                        f.seek(f.getFilePointer()+4); 
                    }
                    break;
                case 80: //Position datagram

                    long pc = f.readUShort();
                    
                    sn = f.readUShort();

                    long lat = f.readUInt();
                    long lon = f.readUInt();

                    long intpq = f.readUShort();
                    long intsv = f.readUShort();
                    long intcv = f.readUShort();
                    long inthd = f.readUShort();
                        
                    f.seek(f.getFilePointer()+8);
                        
                    double lattitude = (double)(1.0 * lat / 20000000);
                    double longitude = (double)(1.0 * lon / 10000000);

                    sv = (double)(1.0 * intsv / 100);
                    cv = (double)(1.0 * intcv / 100);
                    hd = (double)(1.0 * inthd / 100);
                    
                    double uy1 = MercatorProjection.latToY(lattitude);
                    double ux1 = MercatorProjection.lonToX(longitude);
                    
                    //System.out.println(lattitude+" "+longitude);
                    if(!jestpozycja||((Math.abs(ux1 - ux) < 10) && (Math.abs(uy1 - uy) < 10))){
                        jestpozycja = true;
                        
                        if(sx==-100) { sx = ux1; sy = uy1; }
                        ux = ux1;
                        uy = uy1;
//                        System.out.println((ux-sx)+" "+(uy-sy));
                        staretm = tm;
                        
                        tpozycja = new Pozycja();
                        tpozycja.lat = lattitude;
                        tpozycja.lon = longitude;
                    
                        tpozycja.course = cv;
                        tpozycja.speed = sv;
                        tpozycja.heading = hd;
                    }
                    break;
                case 107:
                    //System.out.println("water");                    
                    break;
                default:
                    break;
            }
            // skip the rest of the datagram
            f.seek(filepointer+datagramLength);
        }

        f.close();
    }
}
